#! /usr/bin/env python3

import time, sys, os, json
import paho.mqtt.client as mqtt

sys.path.append(os.path.dirname(os.path.abspath(__file__)))
from config.config import ADDRESS, PORT, CLIENT_ID,SERIAL_NUMBER,PWD,MQTT_SUB_TOPIC,ROS_PUB_TOPIC, MQTT_PUB_TOPIC


class MqttListen:  
    def __init__(self):
        self.pub_topic = ROS_PUB_TOPIC 
        self.broker_address = ADDRESS  
        self.broker_port = PORT
        self.connected = False
        self.mqtt_sub_topic = MQTT_SUB_TOPIC    
        self.mqtt_client = mqtt.Client(client_id=CLIENT_ID)
        self.mqtt_client.username_pw_set(username=SERIAL_NUMBER, password=PWD) 
        self.mqtt_client.on_connect = self.on_connect  
        self.mqtt_client.on_message = self.on_message
        self.mqtt_client.on_disconnect = self.on_disconnect
        self.received = 0
        self.pub = None
        print("开启MQTT监听，收到MQTT信息后发布给机械臂")
  
    def on_connect(self, client, userdata, flags, rc):  
        if rc == 0:
            self.connected = True
            print("Connected to MQTT Broker!")
            client.subscribe(self.mqtt_sub_topic)
            print('subscribe topic: ' + self.mqtt_sub_topic)
        else:
            self.connected = False
            print("Failed to connect, return code %d\n", rc)

    def on_disconnect(self, client, userdata, rc):
        # print("断开链接")
        print("Unexpected disconnection rc = " + str(rc))
        self.connected = False
        pass   
  
    def on_message(self, client, userdata, msg):
        self.received = self.received + 1
        mqtt_msg = msg.payload.decode()
        print('receive message: ' + str(self.received), mqtt_msg)
        # 这里可以添加将MQTT消息发布到ROS话题的代码  
        #if self.pub is not None:
        #    mqtt_msg = msg.payload.decode()
        #    # 监听到消息，进行解析，然后发布机械臂动作
        #    m = self.joint_msg(mqtt_msg=mqtt_msg)
        #    self.pub.publish(m)  
  
    def connect_mqtt(self):  
        self.mqtt_client.connect(self.broker_address, self.broker_port, 60)  
        self.mqtt_client.loop_start()  
  
    def disconnect_mqtt(self):  
        self.mqtt_client.loop_stop()  
        self.mqtt_client.disconnect()
        self.connected = False
  
    def run(self):  
        self.connect_mqtt()
        while True:
            time.sleep(1)


if __name__ == '__main__':
   
    ml = MqttListen() # /1/0001/forward/post 为被控制话题
    ml.run()
